/* Include files */
#include "balloonRobot.h"

using namespace std;
void wander(double speed, ArRobot *thisRobot);

/* Main method */

int main(int argc, char **argv) {
	/* The robot and its devices */
	Aria::init(); //Initialise ARIA library
	ArRobot robot; //Instantiate robot

	ArSonarDevice sonar; //Instantiate its sonar
	robot.addRangeDevice(&sonar); //Add sonar to robot

	ArArgumentParser parser(&argc, argv); //Instantiate argument parser
	ArSimpleConnector connector(&parser); //Instantiate connector
	/* Connection to robot */
	parser.loadDefaultArguments(); //Load default values
	if (!connector.parseArgs()) //Parse connector arguments
	{
		cout << "Unknown settings\n"; //Exit for errors
		Aria::exit(0);
		exit(1);
	}
	if (!connector.connectRobot(&robot)) //Connect to the robot
	{
		cout << "Unable to connect\n"; //Exit for errors
		Aria::exit(0);
		exit(1);
	}
	robot.runAsync(true); //Run in asynchronous mode
	robot.lock(); //Lock robot during set up
	robot.comInt(ArCommands::ENABLE, 1); //Turn on the motors
	robot.unlock(); //Unlock the robot

	//dichiarazioni variabili ed inizializzazione
	int objective = 8;
	int turn = 0; //tiene conto se il robot si è già voltato o meno. 0 = no, 1 = si
	T_Position ballons[MAX_TARGET];
	T_Position hurdles[MAX_HURDLE];
	int numBallons = 0, numHurdles = 0;
	T_Position nearWall;
	T_Position target;

	//inizio ciclo di controllo
	while(checkObjective(objective)==1){
		scanning(ballons,&numBallons,hurdles,&numHurdles);
		if(seeTarget(ballons,numBallons,target) == 0){
			goToBalloon(target);
			explodeBalloon(target,&objective);
			robot.stop();
			turn=0;
		}else{
			turnabout();
			turn=1;
		}
		if(turn==1){
			turn=0;
			searchNearWall(nearWall);
			if(isNearWall(nearWall)==0){
				goToWall(nearWall);
			}
			rotateCamera();
			followWall();
		}
	}


	robot.waitForRunExit(0);
	Aria::exit(0); //Exit Aria
} //End main
